﻿# coding=utf-8
# 2023/4/27: 首个记录，基于旧2022/3/16版本
# 2023/5/5: 枚举基于Enum
# 2023/10/11: 补充部分英文注释
# 2023/11/14: 增加TAI时间类型。增加东北天向速度获取函数enu_speed

import bi_common as bi
from math import pi, sin, cos, atan2
from datetime import datetime, timedelta
from enum import Enum


class GnssImuLocationMode(Enum):
    NO_LOCATION = 0  # 无位置信息
    NORMAL = 1  # 默认模式
    RTK_FIXED = 2  # RTK固定解
    RTK_FLOAT = 3  # RTK浮动解
    RTD = 4  # RTD
    IMU_ONLY = 5  # 仅惯导
    MODIFIED = 6  # 后期修正


class GnssTimeType(Enum):
    UTC = 0  # UTC时间
    GPS = 1  # GPS时间
    TAI = 2  # TAI时间


class SpeedComponents:
    def __init__(self):
        self.east = 0.0  # [kph] East component speed / 东向速度
        self.north = 0.0  # [kph] North component speed / 北向速度
        self.up = 0.0  # [kph] Up component speed / 天向速度


class GnssImuSample:
    def __init__(self):
        self.time = 0.0  # [s] Time offset in session / Session内的相对时间
        self.location_mode = GnssImuLocationMode.NO_LOCATION  # 位置模式
        self.satellite_count_valid = False
        self.satellite_count = 0  # 卫星数量
        self.longitude_valid = False
        self.longitude = 0.0  # [°E] Longitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的经度
        self.latitude_valid = False
        self.latitude = 0.0  # [°N] Latitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的纬度
        self.altitude_valid = False
        self.altitude = 0.0  # [m] Altitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的海拔
        self.antenna_longitude_valid = False
        self.antenna_longitude = 0.0  # [°E] Longitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的经度
        self.antenna_latitude_valid = False
        self.antenna_latitude = 0.0  # [°N] Latitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的纬度
        self.antenna_altitude_valid = False
        self.antenna_altitude = 0.0  # [m] 天线位置海拔Altitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的海拔
        self.horizontal_error_valid = False
        self.horizontal_error = 0.0  # 水平位置误差 m
        self.vertical_error_valid = False
        self.vertical_error = 0.0  # 垂直位置误差 m
        self.speed_valid = False
        self.speed = 0.0  # 车速 kph
        self.orientation_valid = False
        self.orientation = 0.0  # [deg -180~180] 0 as north, CCW is positive / 朝向 CCW为正 北为0
        self.pitch_valid = False
        self.pitch = 0.0  # [deg] Heading down is positive / 俯仰角 车头朝下为正
        self.roll_valid = False
        self.roll = 0.0  # [deg] Right side heading down is positive / 横滚角 右侧朝下为正
        self.yaw_rate_valid = False
        self.yaw_rate = 0.0  # [deg/s] Turning left is positive / 横摆角速度 左转为正
        self.pitch_rate_valid = False
        self.pitch_rate = 0.0  # [deg/s] Heading down is positive / 俯仰角速度 车头朝下为正
        self.roll_rate_valid = False
        self.roll_rate = 0.0  # [deg/s] Right side heading down is positive / 横滚角速度 右侧朝下为正
        self.ax_valid = False
        self.ax = 0.0  # [m/s2] Front side is positive / 纵向加速度 朝前为正
        self.ay_valid = False
        self.ay = 0.0  # [m/s2] Left side is positive / 横向加速度 朝左为正
        self.az_valid = False
        self.az = 0.0  # [m/s2] Top side is positive / 天向加速度 朝上为正
        self.curvature_valid = False
        self.curvature = 0.0  # [1/m] Turning left is positive / 行驶曲率 左转为正
        self.slip_angle_valid = False
        self.slip_angle = 0.0  # [deg] Turning left is positive / 侧偏角 左转时为正
        self.jerk_x_valid = False
        self.jerk_x = 0.0  # [m/s3] Front side is positive / 纵向急动度  朝前为正
        self.jerk_y_valid = False
        self.jerk_y = 0.0  # [m/s3] Left side is positive / 横向急动度 朝左为正
        self.arrival_time_valid = False
        self.arrival_time = 0.0  # [s] Arrival time offset (Relative to the session) / 数据到达时间戳（相对Session开始时间）
        self.gnss_time_type = GnssTimeType.UTC  # 卫星时间类型
        self.gnss_time_valid = False
        self.gnss_time = datetime(1970, 1, 1, 0, 0, 0, 0)  # 卫星时间

    # Query GNSS (satellite) time as UTC / 获取UTC卫星时间
    def gnss_time_utc(self):
        if self.gnss_time_valid:
            if self.gnss_time_type == GnssTimeType.UTC:
                return self.gnss_time
            elif self.gnss_time_type == GnssTimeType.GPS:
                return self.gnss_time + timedelta(seconds=-18)
            elif self.gnss_time_type == GnssTimeType.TAI:
                return self.gnss_time + timedelta(seconds=-37)
        return None
    
    # [kph] Query east-north-up speed / 获取东北天向速度
    def enu_speed(self):
        if self.speed_valid and self.orientation_valid and self.pitch_valid:
            coef = pi / 180
            output = SpeedComponents()
            output.up = -self.speed * sin(self.pitch * coef)
            hSpeed = self.speed * cos(self.pitch * coef)
            output.north = hSpeed * cos(self.orientation * coef)
            output.east = -hSpeed * sin(self.orientation * coef)
            return output
        else:
            return None

    # Convert to general sample for output / 转通用样本，用于样本输出
    def to_general_sample(self, channel):
        output = bi.agency.create_general_sample()
        output.protocol = 'gnssimu-sample-v7@' + str(channel)
        output.time = self.time
        output.significant = 40
        output.values = []
        output.values.append(self.location_mode.value)
        output.values.append(self.longitude if self.longitude_valid else None)
        output.values.append(self.latitude if self.latitude_valid else None)
        output.values.append(self.altitude if self.altitude_valid else None)
        output.values.append(self.speed if self.speed_valid else None)
        output.values.append(self.orientation if self.orientation_valid else None)
        output.values.append(self.pitch if self.pitch_valid else None)
        output.values.append(self.roll if self.roll_valid else None)
        output.values.append(self.yaw_rate if self.yaw_rate_valid else None)
        output.values.append(self.ax if self.ax_valid else None)
        output.values.append(self.ay if self.ay_valid else None)
        output.values.append(self.az if self.az_valid else None)
        output.values.append(self.slip_angle if self.slip_angle_valid else None)
        output.values.append(self.jerk_x if self.jerk_x_valid else None)
        output.values.append(self.jerk_y if self.jerk_y_valid else None)
        output.values.append(self.satellite_count if self.satellite_count_valid else None)
        output.values.append(self.antenna_longitude if self.antenna_longitude_valid else None)
        output.values.append(self.antenna_latitude if self.antenna_latitude_valid else None)
        output.values.append(self.antenna_altitude if self.antenna_altitude_valid else None)
        output.values.append(self.gnss_time_type.value)
        output.values.append(self.arrival_time if self.arrival_time_valid else None)
        output.values.append(self.gnss_time.year if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.month if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.day if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.hour if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.minute if self.gnss_time_valid else None)
        output.values.append(self.gnss_time.second if self.gnss_time_valid else None)
        output.values.append((self.gnss_time.microsecond * 0.001) if self.gnss_time_valid else None)
        output.values.append(self.pitch_rate if self.pitch_rate_valid else None)
        output.values.append(self.roll_rate if self.roll_rate_valid else None)
        output.values.append(self.horizontal_error if self.horizontal_error_valid else None)
        output.values.append(self.vertical_error if self.vertical_error_valid else None)
        output.values.append(self.curvature if self.curvature_valid else None)
        output.values.append(None) # Reserved 34
        output.values.append(None) # Reserved 35
        output.values.append(None) # Reserved 36
        output.values.append(None) # Reserved 37
        output.values.append(None) # Reserved 38
        output.values.append(None) # Reserved 39
        output.values.append(None) # Reserved 40
        return output

    # Set GNSS (satellite) time with validation / 设置GNSS时间，带验证
    def set_gnss_time(self, year, month, day, hour, minute, second, micro):
        self.gnss_time_valid = False
        if year < 2000 or year > 3000 or month < 1 or month > 12 or day < 1 or day > 31 or hour < 0 or hour > 23 or minute < 0 or minute > 59 or second < 0 or second > 59 or micro < 0 or micro > 999999:
            return
        self.gnss_time = datetime(year, month, day, hour, minute, second, micro)
        self.gnss_time_valid = True


def _conv_gnssimu_sample_v4(gs):
    values_count = len(gs.values)
    if values_count != 18:
        return None
    output = GnssImuSample()
    output.time = gs.time

    if gs.values[0] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[0])
    if gs.values[1] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[1])
    output.location_mode = GnssImuLocationMode.NORMAL if output.longitude_valid and output.latitude_valid else GnssImuLocationMode.NO_LOCATION
    if gs.values[2] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[3])
    if gs.values[4] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[4])
    if gs.values[5] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[5])
    if gs.values[6] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[6])
    if gs.values[7] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[7])
    if gs.values[8] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[8])
    if gs.values[9] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[9])
    if gs.values[10] is not None:
        output.az_valid = True
        output.az = float(gs.values[10])
    output.arrival_time_valid = True
    output.arrival_time = gs.time
    if gs.values[11] is not None and gs.values[12] is not None and gs.values[13] is not None and gs.values[14] is not None and gs.values[15] is not None and gs.values[16] is not None and gs.values[17] is not None:
        output.set_gnss_time(int(gs.values[11]), int(gs.values[12]), int(gs.values[13]), int(gs.values[14]), int(gs.values[15]), int(gs.values[16]), int(gs.values[17]) * 1000)
    return output


def _conv_gnssimu_sample_v5(gs):
    values_count = len(gs.values)
    if values_count != 19:
        return None
    output = GnssImuSample()
    output.time = gs.time
    output.location_mode = GnssImuLocationMode(int(gs.values[0])) if gs.values[0] is not None else GnssImuLocationMode.NO_LOCATION
    if gs.values[1] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[1])
    if gs.values[2] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[3])
    if gs.values[4] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[4])
    if gs.values[5] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[5])
    if gs.values[6] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[6])
    if gs.values[7] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[7])
    if gs.values[8] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[8])
    if gs.values[9] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[9])
    if gs.values[10] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[10])
    if gs.values[11] is not None:
        output.az_valid = True
        output.az = float(gs.values[11])
    output.arrival_time_valid = True
    output.arrival_time = gs.time
    if gs.values[12] is not None and gs.values[13] is not None and gs.values[14] is not None and gs.values[15] is not None and gs.values[16] is not None and gs.values[17] is not None and gs.values[18] is not None:
        output.set_gnss_time(int(gs.values[12]), int(gs.values[13]), int(gs.values[14]), int(gs.values[15]), int(gs.values[16]), int(gs.values[17]), int(gs.values[18]) * 1000)
    return output


def _conv_gnssimu_sample_v6(gs):
    values_count = len(gs.values)
    if values_count != 28:
        return None
    output = GnssImuSample()
    output.time = gs.time
    output.location_mode = GnssImuLocationMode(int(gs.values[0])) if gs.values[0] is not None else GnssImuLocationMode.NO_LOCATION
    if gs.values[1] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[1])
    if gs.values[2] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.altitude_valid = True
        output.altitude = float(gs.values[3])
    if gs.values[4] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[4])
    if gs.values[5] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[5])
    if gs.values[6] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[6])
    if gs.values[7] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[7])
    if gs.values[8] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[8])
    if gs.values[9] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[9])
    if gs.values[10] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[10])
    if gs.values[11] is not None:
        output.az_valid = True
        output.az = float(gs.values[11])
    if gs.values[12] is not None:
        output.slip_angle_valid = True
        output.slip_angle = float(gs.values[12])
    if gs.values[13] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[13])
    if gs.values[14] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[14])
    if gs.values[15] is not None:
        output.satellite_count_valid = True
        output.satellite_count = int(gs.values[15])
    if gs.values[16] is not None:
        output.antenna_longitude_valid = True
        output.antenna_longitude = float(gs.values[16])
    if gs.values[17] is not None:
        output.antenna_latitude_valid = True
        output.antenna_latitude = float(gs.values[17])
    if gs.values[18] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[18])
    if gs.values[19] is not None:
        output.gnss_time_type = GnssTimeType(int(gs.values[19]))
    if gs.values[20] is not None:
        output.arrival_time_valid = True
        output.arrival_time = float(gs.values[20])
    if gs.values[21] is not None and gs.values[22] is not None and gs.values[23] is not None and gs.values[24] is not None and gs.values[25] is not None and gs.values[26] is not None and gs.values[27] is not None:
        output.set_gnss_time(int(gs.values[21]), int(gs.values[22]), int(gs.values[23]), int(gs.values[24]), int(gs.values[25]), int(gs.values[26]), int(gs.values[27]) * 1000)
    return output


def _conv_gnssimu_sample_v7(gs):
    values_count = len(gs.values)
    if values_count != 40:
        return None
    output = GnssImuSample()
    output.time = gs.time
    output.location_mode = GnssImuLocationMode(int(gs.values[0])) if gs.values[0] is not None else GnssImuLocationMode.NO_LOCATION
    if gs.values[1] is not None:
        output.longitude_valid = True
        output.longitude = float(gs.values[1])
    if gs.values[2] is not None:
        output.latitude_valid = True
        output.latitude = float(gs.values[2])
    if gs.values[3] is not None:
        output.altitude_valid = True
        output.altitude = float(gs.values[3])
    if gs.values[4] is not None:
        output.speed_valid = True
        output.speed = float(gs.values[4])
    if gs.values[5] is not None:
        output.orientation_valid = True
        output.orientation = float(gs.values[5])
    if gs.values[6] is not None:
        output.pitch_valid = True
        output.pitch = float(gs.values[6])
    if gs.values[7] is not None:
        output.roll_valid = True
        output.roll = float(gs.values[7])
    if gs.values[8] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[8])
    if gs.values[9] is not None:
        output.ax_valid = True
        output.ax = float(gs.values[9])
    if gs.values[10] is not None:
        output.ay_valid = True
        output.ay = float(gs.values[10])
    if gs.values[11] is not None:
        output.az_valid = True
        output.az = float(gs.values[11])
    if gs.values[12] is not None:
        output.slip_angle_valid = True
        output.slip_angle = float(gs.values[12])
    if gs.values[13] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[13])
    if gs.values[14] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[14])
    if gs.values[15] is not None:
        output.satellite_count_valid = True
        output.satellite_count = int(gs.values[15])
    if gs.values[16] is not None:
        output.antenna_longitude_valid = True
        output.antenna_longitude = float(gs.values[16])
    if gs.values[17] is not None:
        output.antenna_latitude_valid = True
        output.antenna_latitude = float(gs.values[17])
    if gs.values[18] is not None:
        output.antenna_altitude_valid = True
        output.antenna_altitude = float(gs.values[18])
    if gs.values[19] is not None:
        output.gnss_time_type = GnssTimeType(int(gs.values[19]))
    if gs.values[20] is not None:
        output.arrival_time_valid = True
        output.arrival_time = float(gs.values[20])
    if gs.values[21] is not None and gs.values[22] is not None and gs.values[23] is not None and gs.values[24] is not None and gs.values[25] is not None and gs.values[26] is not None and gs.values[27] is not None:
        output.set_gnss_time(int(gs.values[21]), int(gs.values[22]), int(gs.values[23]), int(gs.values[24]), int(gs.values[25]), int(gs.values[26]), int(gs.values[27]) * 1000)
    if gs.values[28] is not None:
        output.pitch_rate_valid = True
        output.pitch_rate = float(gs.values[28])
    if gs.values[29] is not None:
        output.roll_rate_valid = True
        output.roll_rate = float(gs.values[29])
    if gs.values[30] is not None:
        output.horizontal_error_valid = True
        output.horizontal_error = float(gs.values[30])
    if gs.values[31] is not None:
        output.vertical_error_valid = True
        output.vertical_error = float(gs.values[31])
    if gs.values[32] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[32])
    return output


def _interpolate_angle(a1, w1, a2, w2):
    deg2rad = pi / 180
    x1 = cos(a1 * deg2rad)
    y1 = sin(a1 * deg2rad)
    x2 = cos(a2 * deg2rad)
    y2 = sin(a2 * deg2rad)
    xo = x1 * w1 + x2 * w2
    yo = y1 * w1 + y2 * w2
    if xo == 0 and yo == 0:
        return None
    return atan2(yo, xo) / deg2rad


def _interpolate_gnssimu_sample(vs1, w1, vs2, w2):
    output = GnssImuSample()
    output.time = bi.time
    output.location_mode = vs1.location_mode if w1 > w2 else vs2.location_mode
    if vs1.location_mode == vs2.location_mode:
        if vs1.longitude_valid and vs2.longitude_valid:
            output.longitude_valid = True
            output.longitude = vs1.longitude * w1 + vs2.longitude * w2
        if vs1.latitude_valid and vs2.latitude_valid:
            output.latitude_valid = True
            output.latitude = vs1.latitude * w1 + vs2.latitude * w2
        if vs1.altitude_valid and vs2.altitude_valid:
            output.altitude_valid = True
            output.altitude = vs1.altitude * w1 + vs2.altitude * w2
        if vs1.antenna_longitude_valid and vs2.antenna_longitude_valid:
            output.antenna_longitude_valid = True
            output.antenna_longitude = vs1.antenna_longitude * w1 + vs2.antenna_longitude * w2
        if vs1.antenna_latitude_valid and vs2.antenna_latitude_valid:
            output.antenna_latitude_valid = True
            output.antenna_latitude = vs1.antenna_latitude * w1 + vs2.antenna_latitude * w2
        if vs1.antenna_altitude_valid and vs2.antenna_altitude_valid:
            output.antenna_altitude_valid = True
            output.antenna_altitude = vs1.antenna_altitude * w1 + vs2.antenna_altitude * w2
    else:
        output.longitude_valid = vs1.longitude_valid if w1 > w2 else vs2.longitude_valid
        output.longitude = vs1.longitude if w1 > w2 else vs2.longitude
        output.latitude_valid = vs1.latitude_valid if w1 > w2 else vs2.latitude_valid
        output.latitude = vs1.latitude if w1 > w2 else vs2.latitude
        output.altitude_valid = vs1.altitude_valid if w1 > w2 else vs2.altitude_valid
        output.altitude = vs1.altitude if w1 > w2 else vs2.altitude
        output.antenna_longitude_valid = vs1.antenna_longitude_valid if w1 > w2 else vs2.antenna_longitude_valid
        output.antenna_longitude = vs1.antenna_longitude if w1 > w2 else vs2.antenna_longitude
        output.antenna_latitude_valid = vs1.antenna_latitude_valid if w1 > w2 else vs2.antenna_latitude_valid
        output.antenna_latitude = vs1.antenna_latitude if w1 > w2 else vs2.antenna_latitude
        output.antenna_altitude_valid = vs1.antenna_altitude_valid if w1 > w2 else vs2.antenna_altitude_valid
        output.antenna_altitude = vs1.antenna_altitude if w1 > w2 else vs2.antenna_altitude
    output.satellite_count_valid = vs1.satellite_count_valid if w1 > w2 else vs2.satellite_count_valid
    output.satellite_count = vs1.satellite_count if w1 > w2 else vs2.satellite_count
    if vs1.speed_valid and vs2.speed_valid:
        output.speed_valid = True
        output.speed = vs1.speed * w1 + vs2.speed * w2
    if vs1.orientation_valid and vs2.orientation_valid:
        output.orientation_valid = True
        output.orientation = _interpolate_angle(vs1.orientation, w1, vs2.orientation, w2)
    if vs1.pitch_valid and vs2.pitch_valid:
        output.pitch_valid = True
        output.pitch = vs1.pitch * w1 + vs2.pitch * w2
    if vs1.roll_valid and vs2.roll_valid:
        output.roll_valid = True
        output.roll = vs1.roll * w1 + vs2.roll * w2
    if vs1.yaw_rate_valid and vs2.yaw_rate_valid:
        output.yaw_rate_valid = True
        output.yaw_rate = vs1.yaw_rate * w1 + vs2.yaw_rate * w2
    if vs1.ax_valid and vs2.ax_valid:
        output.ax_valid = True
        output.ax = vs1.ax * w1 + vs2.ax * w2
    if vs1.ay_valid and vs2.ay_valid:
        output.ay_valid = True
        output.ay = vs1.ay * w1 + vs2.ay * w2
    if vs1.az_valid and vs2.az_valid:
        output.az_valid = True
        output.az = vs1.az * w1 + vs2.az * w2
    if vs1.slip_angle_valid and vs2.slip_angle_valid:
        output.slip_angle_valid = True
        output.slip_angle = vs1.slip_angle * w1 + vs2.slip_angle * w2
    if vs1.jerk_x_valid and vs2.jerk_x_valid:
        output.jerk_x_valid = True
        output.jerk_x = vs1.jerk_x * w1 + vs2.jerk_x * w2
    if vs1.jerk_y_valid and vs2.jerk_y_valid:
        output.jerk_y_valid = True
        output.jerk_y = vs1.jerk_y * w1 + vs2.jerk_y * w2
    output.gnss_time_type = vs1.gnss_time_type
    if vs1.arrival_time_valid and vs2.arrival_time_valid:
        output.arrival_time_valid = True
        output.arrival_time = vs1.arrival_time * w1 + vs2.arrival_time * w2
    if vs1.gnss_time_valid and vs2.gnss_time_valid:
        diff_us = int((vs2.gnss_time - vs1.gnss_time).microseconds * w2 / (w1 + w2))
        output.gnss_time_valid = True
        output.gnss_time = vs1.gnss_time + timedelta(microseconds=diff_us)
    if vs1.pitch_rate_valid and vs2.pitch_rate_valid:
        output.pitch_rate_valid = True
        output.pitch_rate = vs1.pitch_rate * w1 + vs2.pitch_rate * w2
    if vs1.roll_rate_valid and vs2.roll_rate_valid:
        output.roll_rate_valid = True
        output.roll_rate = vs1.roll_rate * w1 + vs2.roll_rate * w2
    if vs1.horizontal_error_valid and vs2.horizontal_error_valid:
        output.horizontal_error_valid = True
        output.horizontal_error = vs1.horizontal_error * w1 + vs2.horizontal_error * w2
    if vs1.vertical_error_valid and vs2.vertical_error_valid:
        output.vertical_error_valid = True
        output.vertical_error = vs1.vertical_error * w1 + vs2.vertical_error * w2
    if vs1.curvature_valid and vs2.curvature_valid:
        output.curvature_valid = True
        output.curvature = vs1.curvature * w1 + vs2.curvature * w2
    return output


# Query GnssImuSample for input / 获取GnssImuSample，用于样本输入
def get_gnssimu_sample(channel):
    vs1 = None
    vs2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v4 = 'gnssimu-sample-v4@' + str(channel)
    protocol_id_v5 = 'gnssimu-sample-v5@' + str(channel)
    protocol_id_v6 = 'gnssimu-sample-v6@' + str(channel)
    protocol_id_v7 = 'gnssimu-sample-v7@' + str(channel)
    if protocol_id_v7 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v7]
        vs1 = _conv_gnssimu_sample_v7(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v7(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v6 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v6]
        vs1 = _conv_gnssimu_sample_v6(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v6(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v5 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v5]
        vs1 = _conv_gnssimu_sample_v5(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v5(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v4 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v4]
        vs1 = _conv_gnssimu_sample_v4(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_gnssimu_sample_v4(pair.sample2)
        w2 = pair.weight2
    if vs1 is not None and vs2 is not None:
        return _interpolate_gnssimu_sample(vs1, w1, vs2, w2)
    return None
